/*
 * rviz_motion_state.h
 *
 *  Created on: 16.09.2013
 *      Author: alex
 */

#ifndef RVIZ_MOTION_STATE_H_
#define RVIZ_MOTION_STATE_H_

#include <stdint.h>
#include <ros/publisher.h>
#include <bullet/LinearMath/btTransform.h>
#include <bullet/LinearMath/btVector3.h>
#include <bullet/LinearMath/btMotionState.h>
#include <std_msgs/ColorRGBA.h>

namespace bullet_rviz_test
{

class RvizMotionState: public btMotionState
{
public:
  RvizMotionState(const btVector3 & box_half_extents, const btTransform & world_transform, int32_t id);
  virtual ~RvizMotionState();

  virtual void getWorldTransform(btTransform & world_transform) const;
  virtual void setWorldTransform(const btTransform & world_transform);

  void publishMarker();

  void setColor(const std_msgs::ColorRGBA & color);
  void setColor(uint32_t rgba);

private:
  btVector3 box_half_extents_;
  btTransform world_transform_;
  int32_t id_;
  std_msgs::ColorRGBA color_;
  ros::Publisher marker_publisher_;
};

}

#endif /* RVIZ_MOTION_STATE_H_ */
